function Rz = Rotz(theta)
    Rz = zeros(3,3);
    Rz = [cos(theta)  -sin(theta)   0;
          sin(theta)  cos(theta)    0;
            0            0          1];
end